package com.wltt.product.frfront.service;

import com.wltt.product.frfront.config.exception.BusinessException;
import com.wltt.product.frfront.config.javafx.utils.AlertUtil;
import com.wltt.product.frfront.config.robot.WlttRobot;
import com.wltt.product.frfront.enums.CoordinateSystemEnum;
import com.wltt.product.frfront.executors.JointMoveExecutor;
import com.wltt.product.frfront.executors.PoseMoveExecutor;
import com.wltt.product.frfront.executors.WorkStealingPool;
import com.wltt.product.frfront.holder.RobotHolder;
import com.wltt.product.frfront.mapper.RobotMapper;
import com.wltt.product.frfront.model.ResultData;
import com.wltt.product.frfront.model.po.RobotPO;
import com.wltt.product.frfront.model.vo.*;
import com.wltt.product.frfront.model.vo.position.FrDescTran;
import com.wltt.product.frfront.model.vo.position.FrJointPos;
import com.wltt.product.frfront.service.base.BaseServiceImpl;
import fairino.*;
import jakarta.annotation.Resource;
import javafx.scene.control.Alert;
import org.springframework.http.HttpStatus;
import org.springframework.stereotype.Service;
import org.springframework.transaction.annotation.Transactional;

import java.util.ArrayList;
import java.util.List;

@Service(value = "robotService")
public class RobotServiceImpl extends BaseServiceImpl<RobotPO, Long> implements RobotService {

    private RobotMapper robotMapper;


    @Resource
    private WlttRobot robot;

    //构造方法
    public RobotServiceImpl(RobotMapper robotMapper) {
        super(robotMapper);
        this.robotMapper = robotMapper;
    }

    /**
     * 获取位置信息
     *
     * @return
     */
    @Override
    public ResultData getPosition() {
//        // 模拟返回
//        FrDescTran descTran  = new FrDescTran();
//        descTran.setX(0.0);
//        descTran.setRx(0.0);
//        descTran.setY(0.0);
//        descTran.setRy(0.0);
//        descTran.setZ(0.0);
//        descTran.setRz(0.0);
//
//        FrJointPos jointPos = new FrJointPos();
//        jointPos.setJ1(0.0);
//        jointPos.setJ2(0.0);
//        jointPos.setJ3(0.0);
//        jointPos.setJ4(0.0);
//        jointPos.setJ5(0.0);
//        jointPos.setJ6(0.0);
//
//        GetPositionRSP getPositionRSP = new GetPositionRSP();
//        getPositionRSP.setDescTran(descTran);
//        getPositionRSP.setJointPos(jointPos);
//        return ResultData.success(getPositionRSP);
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }

        JointPos jointPos = new JointPos();
        robot.GetActualJointPosDegree(jointPos);
        FrJointPos frjointPos = new FrJointPos();
        frjointPos.setJ1(jointPos.getJ1());
        frjointPos.setJ2(jointPos.getJ2());
        frjointPos.setJ3(jointPos.getJ3());
        frjointPos.setJ4(jointPos.getJ4());
        frjointPos.setJ5(jointPos.getJ5());
        frjointPos.setJ6(jointPos.getJ6());

        DescPose desc_pos = new DescPose();
        robot.GetActualTCPPose(desc_pos);
        FrDescTran frDescTran = new FrDescTran();
        frDescTran.setX(desc_pos.tran.x);
        frDescTran.setRx(desc_pos.rpy.rx);
        frDescTran.setY(desc_pos.tran.y);
        frDescTran.setRy(desc_pos.rpy.ry);
        frDescTran.setZ(desc_pos.tran.z);
        frDescTran.setRz(desc_pos.rpy.rz);

        GetPositionRSP getPositionRSP = new GetPositionRSP();
        getPositionRSP.setDescTran(frDescTran);
        getPositionRSP.setJointPos(frjointPos);
        return ResultData.success(getPositionRSP);
    }

    /**
     * 获取单次运行最大值
     *
     * @return
     */
    @Override
    public ResultData getMaxStep() {
        RobotPO robotPO = robotMapper.queryById(1L);
        GetMaxStepRSP getMaxStepRSP = new GetMaxStepRSP();
        getMaxStepRSP.setMaxStep(robotPO.getMaxStep());
        return ResultData.success(getMaxStepRSP);
    }

    /**
     * 设置单次运行最大值
     *
     * @return
     */
    @Override
    @Transactional
    public ResultData setMaxStep(SetMaxStepREQ setMaxStepREQ) {
        if (setMaxStepREQ.getMaxStep() == null || setMaxStepREQ.getMaxStep() <= 0) {
            throw new BusinessException("单次运行最大值无效", HttpStatus.BAD_REQUEST);
        }
        RobotPO robotPO = robotMapper.queryById(1L);
        robotPO.setMaxStep(setMaxStepREQ.getMaxStep());
        robotMapper.updateById(robotPO);
        return ResultData.success();
    }

    /**
     * 获取坐标系
     *
     * @return
     */
    @Override
    public ResultData coordinateSystem() {
        List<CoordinateSystemRSP> coordinateSystemRSPList = new ArrayList<>();
        CoordinateSystemEnum[] values = CoordinateSystemEnum.values();
        for (CoordinateSystemEnum coordinateSystemEnum : values) {
            CoordinateSystemRSP rsp = new CoordinateSystemRSP();
            rsp.setType(coordinateSystemEnum.getCode());
            rsp.setName(coordinateSystemEnum.getDesc());
            coordinateSystemRSPList.add(rsp);
        }
        return ResultData.success(coordinateSystemRSPList);
    }

    /**
     * 关节控制
     *
     * @return
     */
    @Override
    public ResultData JointMove(JointMoveREQ jointMoveREQ) {
        if (RobotHolder.isBusy()) {
            throw new BusinessException("当前机器人忙", HttpStatus.BAD_REQUEST);
        }
        if (jointMoveREQ.getMaxStep() == null || jointMoveREQ.getMaxStep() <= 0) {
            throw new BusinessException("单次运行最大值无效", HttpStatus.BAD_REQUEST);
        }
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        JointMoveExecutor jointMoveExecutor = new JointMoveExecutor(robot, jointMoveREQ);
        WorkStealingPool.execute(jointMoveExecutor);
        return ResultData.success();
    }

    /**
     * 位姿控制
     *
     * @param poseMoveREQ
     * @return
     */
    @Override
    public ResultData poseMove(PoseMoveREQ poseMoveREQ) {
        if (RobotHolder.isBusy()) {
            throw new BusinessException("当前机器人忙", HttpStatus.BAD_REQUEST);
        }
        if (poseMoveREQ.getMaxStep() == null || poseMoveREQ.getMaxStep() <= 0) {
            throw new BusinessException("单次运行最大值无效", HttpStatus.BAD_REQUEST);
        }
        if (poseMoveREQ.getCoordinateSystemType() == null) {
            throw new BusinessException("未指定坐标系", HttpStatus.BAD_REQUEST);
        }
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        PoseMoveExecutor poseMoveExecutor = new PoseMoveExecutor(robot, poseMoveREQ);
        WorkStealingPool.execute(poseMoveExecutor);
        return ResultData.success();
    }

    /**
     * 运动停止
     *
     * @return
     */
    @Override
    public ResultData moveStop() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.ImmStopJOG();
        RobotHolder.setRobotNoMoving();
        return ResultData.success();
    }

    /**
     * 清楚报警
     *
     * @return
     */
    @Override
    public ResultData cleanWarn() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.ResetAllError();
        return ResultData.success();
    }

    /**
     * 获取运行信息
     *
     * @return
     */
    @Override
    public ResultData runInfo() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        int[] maincode = new int[1];
        int[] subcode = new int[1];
        int errcode = robot.GetRobotErrorCode(maincode, subcode);
        if (errcode == 0) {
            return ResultData.success("运行正常");
        } else {
            String errorMsg = RobotHolder.ERROR_MAP.get(errcode);
            if (errorMsg != null) {
                return ResultData.success(errorMsg);
            }
            return ResultData.success("未登记的错误信息，错误码：" + errcode);
        }
    }

    /**
     * 获取工具坐标号
     *
     * @return
     */
    @Override
    public ResultData getToolCoordinateNum() {
        RobotPO robotPO = robotMapper.queryById(1L);
        GetToolCoordinateNumRSP rsp = new GetToolCoordinateNumRSP();
        rsp.setToolCoordinateNum(robotPO.getToolCoordinateNum());
        return ResultData.success(rsp);
    }

    /**
     * 设置工具坐标号
     *
     * @param setToolCoordinateNumREQ
     * @return
     */
    @Override
    public ResultData setToolCoordinateNum(SetToolCoordinateNumREQ setToolCoordinateNumREQ) {
        if (setToolCoordinateNumREQ.getToolCoordinateNum() == null || setToolCoordinateNumREQ.getToolCoordinateNum() < 0 || setToolCoordinateNumREQ.getToolCoordinateNum() > 14) {
            throw new BusinessException("范围：0-14", HttpStatus.BAD_REQUEST);
        }
        RobotPO robotPO = robotMapper.queryById(1L);
        robotPO.setToolCoordinateNum(setToolCoordinateNumREQ.getToolCoordinateNum());
        robotMapper.updateById(robotPO);
        return ResultData.success();
    }

    /**
     * 获取工件坐标号
     *
     * @return
     */
    @Override
    public ResultData getWorkpieceCoordinateNum() {
        RobotPO robotPO = robotMapper.queryById(1L);
        GetWorkpieceCoordinateNumRSP rsp = new GetWorkpieceCoordinateNumRSP();
        rsp.setWorkpieceCoordinateNum(robotPO.getWorkpieceCoordinateNum());
        return ResultData.success(rsp);
    }

    /**
     * 设置工件坐标号
     *
     * @param setWorkpieceCoordinateNumREQ
     * @return
     */
    @Override
    public ResultData setWorkpieceCoordinateNum(SetWorkpieceCoordinateNumREQ setWorkpieceCoordinateNumREQ) {
        if (setWorkpieceCoordinateNumREQ.getWorkpieceCoordinateNum() == null || setWorkpieceCoordinateNumREQ.getWorkpieceCoordinateNum() < 0 || setWorkpieceCoordinateNumREQ.getWorkpieceCoordinateNum() > 14) {
            throw new BusinessException("范围：0-14", HttpStatus.BAD_REQUEST);
        }
        RobotPO robotPO = robotMapper.queryById(1L);
        robotPO.setWorkpieceCoordinateNum(setWorkpieceCoordinateNumREQ.getWorkpieceCoordinateNum());
        robotMapper.updateById(robotPO);
        return ResultData.success();
    }
}
